#ifndef __EM_PLANNER_PLANNER_HPP__
#define __EM_PLANNER_PLANNER_HPP__

#include <iostream>
#include <vector>
#include <array>
#include <cmath>
#include <algorithm>
#include <Eigen/Dense>

#include <ros/ros.h>
#include <nav_msgs/Path.h>
#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/Twist.h>
#include <visualization_msgs/Marker.h>
#include <gazebo_msgs/ModelStates.h>
#include <tf/transform_listener.h>

#include <boost/bind.hpp>
#include <boost/thread.hpp>

#include "./em_utils.hpp"
#include "./referenceline_provider.hpp"
#include "./location.hpp"
#include "./obstacle.hpp"
#include "./qp_solver.hpp"
#include "./speed_planner.hpp"
#include "./matplotlibcpp.h"

#include "em_planner/SpeedPlannerInfo.h"

using namespace std;
namespace plt = matplotlibcpp;

struct DPParams
{   
    int end_s;                          // 五次多项式终点的s
    int end_l;                          // 五次多项式终点的l
    vector<double> QuinticPoly;         // 五次多项式的参数
};


class Planner
{
private:

    ros::NodeHandle nh;
    ros::Publisher trajectory_pub;
    ros::Publisher trajectory_marker_pub;
    ros::Publisher debug_pub_;
    ros::Publisher speed_planner_info_pub;

    ReferenceLineProvider referenceline_provider;       // 参考线模块
    LocationInfo host_info;                             // 定位模块
    GenObstacle obs_info;                               // 障碍物生成模块（感知）

    vector<TraPoint> trajectory;
    vector<TraPoint> pre_trajectory;

    SpeedPlanner *s_planner;

    int stitch_num;                     // 拼接的轨迹长度
    /* qp 优化 */
    QPSolver *qp_solver;
    /* dp 参数 */
    double plan_dis;                    // 需要规划的路径长度
    double interval;
    int dp_row_num;
    int dp_col_num;
    double delta_s;
    double delta_l;
    double W_cost_ref;                  // 动态规划参考线cost
    double W_cost_collision;            // 动态规划碰撞cost
    double W_alpha_collision;
    double W_cost_smooth_dl;
    double W_cost_smooth_ddl;
    double W_cost_smooth_dddl;

    /* qp 参数 */
    double qp_cost_l = 5;
    double qp_cost_dl = 40;     // 40000
    double qp_cost_ddl = 30;    // 300
    double qp_cost_dddl = 15;
    double qp_cost_center = 120;     // 1200
    double qp_cost_end_l = 40;
    double qp_cost_end_dl = 40;
    double qp_cost_end_ddl = 40;
    
    
public:
    Planner(/* args */);
    ~Planner();

    bool getParams();                   // 读取参数配置

    void run();
    /**
     * 规划器主函数，返回一条规划的轨迹
     * @param trajectory: 返回规划的轨迹信息
    */
    bool runPlanner(vector<TraPoint> &trajectory);
    void trajectory_pub_fun();
    // 获取规划起点
    bool calc_plan_start(vector<TraPoint> &pre_plan_tra, Location host, 
                        Location &plan_start, vector<TraPoint> &stitch_tra);
    /**
     * 将ros::Time 对象转化为毫秒
    */
    uint32_t rosTime2MSec(ros::Time time) {
        // 将秒减去一部分后化为毫秒
        uint32_t sec_msec = (time.sec - 1.7e9) * 1000;
        // 将纳秒部分化为毫秒
        uint32_t nsec_msec = time.nsec / 1e6;
        return sec_msec + nsec_msec; 
    }

    /**
     * 发布 marker 函数，用于debug
    */
    void plan_start_pub(double x, double y, vector<Location> &list_point);
    void marker_line_strip_pub_fun(const vector<TraPoint> &list, const string ns, 
                                const int32_t id, const Color color);

    /**
     * 动态规划流程
     * @param plan_start_sl: 自然坐标系中规划起点坐标
     * @param row: 行数(横向规划范围是 7 米)
     * @param col: 列数(纵向往前规划范围是 50 米)
     * @param plan_tra: 规划的路径（粗解）
    */
    bool dp_decision(const Location &plan_start, const SLPoint &plan_start_sl,
                    const ReferencePoint &plan_start_proj, const int plan_start_m_index,
                    const vector<ReferencePoint> &cur_ref_line, 
                    const vector<SLPoint> &obs_vec_sl, vector<SLPoint> &sl_plan_tra);

    vector<double> calculateQuinticParams(const SLPoint &start_sl, const SLPoint &end_sl);

    // 动态规划代价函数
    double calculateCost(const SLPoint &start_sl, const SLPoint &end_sl, const vector<SLPoint> &obs_vec_sl);

    // 障碍物代价
    double calcObsCost(double W_cost_collision, Eigen::ArrayXd &square_d);

    /**
     * 将第 j 列第 i 行的点转化为 SL自然坐标系中的坐标
    */
    SLPoint IJ2SL(const SLPoint &plan_start_sl, int cur_col, int cur_row);

    bool IJ2XY(const ReferencePoint &plan_start_proj, 
                const int plan_start_m_index, const vector<ReferencePoint> cur_ref_line, 
                const int interval_offset, const SLPoint &new_sl, Location &xy_point);

    bool IJ2XY(const SLPoint &plan_start_sl,
                const ReferencePoint &plan_start_proj, const int plan_start_m_index, 
                const vector<ReferencePoint> cur_ref_line, 
                const int i, const int j, Location &xy_point);

    bool traSL2XY(const ReferencePoint &plan_start_proj, const int plan_start_m_index, 
                       const vector<ReferencePoint> cur_ref_line, 
                       const vector<SLPoint> &plan_tra_sl, vector<Location> &path_xy);
    /**
     * 二次规划函数
    */
    bool qp_optimal(const SLPoint &plan_start_sl, 
                         const vector<SLPoint> &obs_vec_sl,
                         vector<SLPoint> &plan_tra_sl);
    bool getLBoundary(const vector<SLPoint> &dp_path_sl, const vector<SLPoint> &obs_vec_sl, 
                  vector<double> &l_min, vector<double> &l_max);
    int getNearestIndex(const vector<SLPoint> &dp_path_sl, const double &obs_sl_s);
    
};


#endif